﻿using System;
using System.Threading;
using System.Drawing;

using Microsoft.Kinect;

using Microsoft.Xna.Framework;

using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.CvEnum;

using KinectMenagerNamespace.KinectGestureParts;

namespace KinectMenagerNamespace
{
    public enum FistState
    {
        Open, Close, Undefined, NotTracked
    }
    
    public enum FistTrackingMode
    {
        None, Left, Right, LeftPositionOnly, RightPositionOnly, BothPositionOnly
    }

    public enum FistType
    {
        Left, Right
    }

    public class KinectMenager : IDisposable
    {
        #region Fields

        private KinectSensor kinect;
        private CoordinateMapper coordMapper;
        private Skeleton activeSkeleton;

        private KinectGestureRecognizer gestureRecognizer;
        public KinectGestureRecognizer GestureRecognizer
        {
            get { return gestureRecognizer; }
        }

        private bool kinectIsConnected;
        public bool KinectIsConnected
        {
            get { return kinectIsConnected; }
        }

        private Skeleton[] skeletons = null;
        private short[] depthData = null;
        private byte[] colorData = null;
        
        private int skeletonCounter = 1;
        private int SkeletonCounter
        {
            get { return skeletonCounter; }
            set
            {
                if (value > 100)
                {
                    skeletonCounter = 0;
                }
                else
                {
                    skeletonCounter = value;
                }
            }
        }

        private int depthCounter = 1;
        private int DepthCounter
        {
            get { return depthCounter; }
            set
            {
                if (value > 100)
                {
                    depthCounter = 0;
                }
                else
                {
                    depthCounter = value;
                }
            }
        }

        private float leftFistArea, rightFistArea;
        public float RightFistArea
        {
            get { return rightFistArea; }
        }
        public float LeftFistArea
        {
            get { return leftFistArea; }
        }

        private FistState leftFistState = FistState.Undefined, rightFistState = FistState.Undefined;
        public FistState RightFistState
        {
            get { return rightFistState; }
        }
        public FistState LeftFistState
        {
            get { return leftFistState; }
        }

        private int leftFistLastStatesCounter = 0;
        private int LeftFistLastStatesCounter
        {
            get { return leftFistLastStatesCounter; }
            set {
                if (value >= leftFistLastStates.Length)
                {
                    leftFistLastStatesCounter = 0;
                }
                else
                {
                    leftFistLastStatesCounter = value;
                }
            }
        }

        private int rightFistLastStatesCounter = 0;
        private int RightFistLastStatesCounter
        {
            get { return rightFistLastStatesCounter; }
            set {
                if (value >= rightFistLastStates.Length)
                {
                    rightFistLastStatesCounter = 0;
                }
                else
                {
                    rightFistLastStatesCounter = value;
                }
            }
        }

        private FistState[] leftFistLastStates;
        private FistState[] rightFistLastStates;

        private int leftFistLastStatesLength;
        private int rightFistLastStatesLength;

        private FistTrackingMode activeFistTrackingMode;
        public FistTrackingMode ActiveFistTrackingMode
        {
            get { return activeFistTrackingMode; }
            set
            {
                if (calibrationFinish)
                {
                    if (value == FistTrackingMode.Left || 
                        value == FistTrackingMode.LeftPositionOnly)
                    {
                        if (!leftFistCalibrationFinish)
                        {
                            calibrationFinish = false;
                        }
                    }

                    if (value == FistTrackingMode.Right || 
                        value == FistTrackingMode.RightPositionOnly)
                    {
                        if (!rightFistCalibrationFinish)
                        {
                            calibrationFinish = false;
                        }
                    }

                    if (value == FistTrackingMode.BothPositionOnly || 
                        value == FistTrackingMode.None)
                    {
                        calibrationFinish = false;
                    }

                    activeFistTrackingMode = value;
                }
            }
        }

        private int whatIsCalibrated = 1; //1 - dalekoOpen, 2 - dalekoClose, 3 - bliskoOpen, 4 - bliskoClose
        public int WhatIsCalibrated
        {
            get { return whatIsCalibrated; }
        }

        private int lastSkeletonFrame = 0;
        private int lastDepthFrame = 0;

        private bool findSkeleton = false;
        public bool FindSkeleton
        {
            get { return findSkeleton; }
        }

        private bool leftFistCalibrationFinish = false;
        public bool LeftFistCalibrationFinish
        {
            get { return leftFistCalibrationFinish; }
        }

        private bool rightFistCalibrationFinish = false;
        public bool RightFistCalibrationFinish
        {
            get { return rightFistCalibrationFinish; }
        }

        private bool calibrationFinish = false;
        public bool CalibrationFinish
        {
            get { return calibrationFinish; }
        }

        private bool rightFistCalibrationActive = false;
        public bool RightFistCalibrationActive
        {
            get { return rightFistCalibrationActive; }
        }

        private bool leftFistCalibrationActive = false;
        public bool LeftFistCalibrationActive
        {
            get { return leftFistCalibrationActive; }
        }

        private int dxHalf = 80, dyHalf = 60;
        public int HandRectHalfX
        {
            get { return dxHalf; }
            set { dxHalf = value; }
        }
        public int HandRectHalfY
        {
            get { return dyHalf; }
            set { dyHalf = value; }
        }

        private bool newRightFrame = false, newLeftFrame = false;

        public float minRightZ = 1000, maxRightZ = 0, farOpenedRightArea, farClosedRightArea, nearOpenedRightArea, nearClosedRightArea;
        public float minLeftZ = 1000, maxLeftZ = 0, farOpenedLeftArea, farClosedLeftArea, nearOpenedLeftArea, nearClosedLeftArea;   
                
        private float openedRightA, openedRightB, closedRightA, closedRightB;
        private float openedLeftA, openedLeftB, closedLeftA, closedLeftB;
        private float rightHandZ, leftHandZ;

        private Bitmap leftFistAreaTexture = null, rightFistAreaTexture = null;
        public Bitmap RightFistAreaTexture
        {
            get { return rightFistAreaTexture; }
        }
        public Bitmap LeftFistAreaTexture
        {
            get { return leftFistAreaTexture; }
        }

        private int calibrationBufferLength = 200;
        public int CalibrationBufferLength
        {
            get { return calibrationBufferLength; }
            set 
            {
                if (calibrationFinish && value > CalibrationBufferDrop * 2 + 1)
                {
                    calibrationBufferLength = value;
                    calibrationBuffer = new float[calibrationBufferLength];
                } 
            }
        }

        private int calibrationBufferDrop = 20;
        public int CalibrationBufferDrop
        {
            get { return calibrationBufferDrop; }
            set 
            {
                if (value * 2 < calibrationBufferLength)
                {
                    calibrationBufferDrop = value; 
                }
            }
        }

        private float[] calibrationBuffer;
        private int calibrationBufferCounter = 0;
        public int CalibrationBufferCounter
        {
            get { return calibrationBufferCounter; }
        }

        private TimeSpan betweenCalibrationTime = new TimeSpan(0, 0, 3);
        public TimeSpan BetweenCalibrationTime
        {
            get { return betweenCalibrationTime; }
            set 
            {
                if (calibrationFinish)
                {
                    betweenCalibrationTime = value;
                }                 
            }
        }
        private DateTime lastCalibraionTime;

        private float minX, minY, minZ, maxX, maxY, maxZ; 
        private float positionRightAX, positionRightBX, positionRightAY, positionRightBY, positionRightAZ, positionRightBZ;
        private float positionLeftAX, positionLeftBX, positionLeftAY, positionLeftBY, positionLeftAZ, positionLeftBZ;
        private float realRightMinX, realRightMinY, realRightMinZ, realRightMaxX, realRightMaxY, realRightMaxZ;
        private float realLeftMinX, realLeftMinY, realLeftMinZ, realLeftMaxX, realLeftMaxY, realLeftMaxZ;

        private bool rotationGestureEnable = false;

        public bool RotationGestureEnable
        {
            get { return rotationGestureEnable; }
            set { rotationGestureEnable = value; }
        }

        private bool translationGestureEnable = false;

        public bool TranslationGestureEnable
        {
            get { return translationGestureEnable; }
            set { translationGestureEnable = value; }
        }

        private TimeSpan rotationGestureStabilizationTime = new TimeSpan(0, 0, 0, 0, 1000);
        private DateTime lastRotationGestureStableTime;
        private TimeSpan rotationGestureInactiveTime = new TimeSpan(0, 0, 1);
        private DateTime lastRotationGestureAbleTime = DateTime.Now;
        private Vector3 lastLeftHandPositionRG = new Vector3(), lastRightHandPositionRG = new Vector3();
        private Vector3 leftHandPositionRG, rightHandPositionRG, sphereCenterRG;
        
        private bool onlyx = false, onlyy = false, onlyz = false;
        public bool RotateOnlyZ
        {
            get { return onlyz; }
        }
        public bool RotateOnlyY
        {
            get { return onlyy; }
        }
        public bool RotateOnlyX
        {
            get { return onlyx; }
        }

        private float lastax, lastay, lastaz;

        private TimeSpan translationGestureStabilizationTime = new TimeSpan(0, 0, 0, 0, 900);
        private DateTime lastTranslationGestureStableTime = DateTime.Now;
        private TimeSpan translationGestureInactiveTime = new TimeSpan(0, 0, 1);
        private DateTime lastTranslationGestureAbleTime = DateTime.Now;

        private bool mayBeTranslationChange = false;
        private Vector3 rightHandPositionTG, leftHandPositionTG;

        private bool scaleGestureEnable;
        public bool ScaleGestureEnable
        {
            get { return scaleGestureEnable; }
            set { scaleGestureEnable = value; }
        }

        private TimeSpan scaleGestureStabilizationTime = new TimeSpan(0, 0, 0, 0, 1500);
        private DateTime lastScaleGestureStableTime = DateTime.Now;
        private TimeSpan scaleGestureInactiveTime = new TimeSpan(0, 0, 1);
        private DateTime lastScaleGestureAbleTime = DateTime.Now;
        private float distanceSG, lastDistance;

        private KinectGesture leftHandBelowShoulder, leftHandAboveHead;

        private Thread leftFistCalculator, rightFistCalculator, statusChecker;

        private KinectSpeechRecognizer speechRecognizer;
        public KinectSpeechRecognizer SpeechRecognizer
        {
            get { return speechRecognizer; }
        }

        #endregion

        #region Constructor

        public KinectMenager
            (
                FistTrackingMode whichHand,
                int statesBufferLength = 10,
                int handRectHalfX = 80,
                int handRectHalfY = 60,
                int calibrationBufferLength = 200,
                int calibrationBufferDrop = 20,
                int betweeneCalibrationSeconds = 3
            )
        {
            kinect = null;

            activeFistTrackingMode = whichHand;

            leftFistLastStatesLength = statesBufferLength;
            rightFistLastStatesLength = statesBufferLength;

            dxHalf = handRectHalfX;
            dyHalf = handRectHalfY;

            this.calibrationBufferLength = calibrationBufferLength;
            this.calibrationBufferDrop = calibrationBufferDrop;

            betweenCalibrationTime = new TimeSpan(0, 0, betweeneCalibrationSeconds);

            gestureRecognizer = new KinectGestureRecognizer();
            calibrationBuffer = new float[calibrationBufferLength];

            leftFistLastStates = new FistState[leftFistLastStatesLength];
            rightFistLastStates = new FistState[rightFistLastStatesLength];

            for (int i = 0; i < leftFistLastStates.Length; i++)
            {
                leftFistLastStates[i] = FistState.Undefined;
            }
            for (int i = 0; i < rightFistLastStates.Length; i++)
            {
                rightFistLastStates[i] = FistState.Undefined;
            }

            statusChecker = new Thread(new ThreadStart(CheckStatus));

            KinectGesturePart[] parts10 = new KinectGesturePart[1];
            parts10[0] = new LeftHandBelowShoulder();

            leftHandBelowShoulder = new KinectGesture(parts10, new TimeSpan(0, 0, 10, 10));
            GestureRecognizer.AddGesture(leftHandBelowShoulder);
            //leftHandBelowShoulder.Enable();
            leftHandBelowShoulder.OnGestureSucces += new GestureHandler(leftHandBelowShoulder_OnGestureSucces);

            KinectGesturePart[] parts11 = new KinectGesturePart[1];
            parts11[0] = new LeftHandAboveHead();

            leftHandAboveHead = new KinectGesture(parts11, new TimeSpan(0, 0, 10, 10));
            GestureRecognizer.AddGesture(leftHandAboveHead);
            //leftHandAboveHead.Enable();
            leftHandAboveHead.OnGestureSucces += new GestureHandler(leftHandAboveHead_OnGestureSucces);

            speechRecognizer = new KinectSpeechRecognizer();

            //speechRecognizer.OK += new EventHandler(speechRecognizer_OK);
            //speechRecognizer.MOVE += new EventHandler(speechRecognizer_MOVE);

            foreach (KinectSensor possibleKinect in KinectSensor.KinectSensors)
            {
                if (possibleKinect.Status == KinectStatus.Connected)
                {
                    kinect = possibleKinect;
                    coordMapper = new CoordinateMapper(kinect);

                    TransformSmoothParameters smoothingParam = new TransformSmoothParameters();
                    {
                        smoothingParam.Smoothing = 0.5f;
                        smoothingParam.Correction = 0.1f;
                        smoothingParam.Prediction = 0.5f;
                        smoothingParam.JitterRadius = 0.1f;
                        smoothingParam.MaxDeviationRadius = 0.1f;
                    };

                    kinect.SkeletonStream.Enable();//smoothingParam);
                    kinect.ColorStream.Disable();
                    kinect.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);

                    skeletons = new Skeleton[kinect.SkeletonStream.FrameSkeletonArrayLength];
                    depthData = new short[kinect.DepthStream.FramePixelDataLength];

                    kinect.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(kinect_AllFramesReady);
                    //kinect.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(kinect_DepthFrameReady);
                    //kinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(kinect_SkeletonFrameReady);

                    kinect.Start();

                    speechRecognizer.SetAudioSource(kinect.AudioSource);
                    break;
                }
            }
        }        

        #endregion

        #region Public Methods
        
        public void SetWorkArea(float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
        {
            this.minX = minX;
            this.minY = minY;
            this.minZ = minZ;
            this.maxX = maxX;
            this.maxY = maxY;
            this.maxZ = maxZ;

            if (rightFistCalibrationFinish)
            {
                positionRightAX = (minX - maxX) / (realRightMinX - realRightMaxX);
                positionRightBX = maxX - positionRightAX * realRightMaxX;
                positionRightAY = (minY - maxY) / (realRightMinY - realRightMaxY);
                positionRightBY = maxY - positionRightAY * realRightMaxY;
                positionRightAZ = (minZ - maxZ) / (realRightMinZ - realRightMaxZ);
                positionRightBZ = maxZ - positionRightAZ * realRightMaxZ;
            }

            if (leftFistCalibrationFinish)
            {
                positionLeftAX = (minX - maxX) / (realLeftMinX - realLeftMaxX);
                positionLeftBX = maxX - positionLeftAX * realLeftMaxX;
                positionLeftAY = (minY - maxY) / (realLeftMinY - realLeftMaxY);
                positionLeftBY = maxY - positionLeftAY * realLeftMaxY;
                positionLeftAZ = (minZ - maxZ) / (realLeftMinZ - realLeftMaxZ);
                positionLeftBZ = maxZ - positionLeftAZ * realLeftMaxZ;
            }
        }

        public void Initialize()
        {
            statusChecker.Start();
        }

        public void Dispose()
        {
            statusChecker.Abort();

            if (kinectIsConnected)
            {
                kinect.Stop();
                kinect.Dispose();

                leftFistCalculator.Abort();
                rightFistCalculator.Abort();
            }            
        }

        public void ReCalibrate()
        {
            calibrationFinish = false;
            findSkeleton = false;
            leftFistCalibrationFinish = false;
            rightFistCalibrationFinish = false;
        }

        public void ReCalibrate(FistTrackingMode trackingMode)
        {
            calibrationFinish = false;
            findSkeleton = false;
            leftFistCalibrationFinish = false;
            rightFistCalibrationFinish = false;

            ActiveFistTrackingMode = trackingMode;
        }

        public void Update()
        {
            if (activeSkeleton != null && lastSkeletonFrame != SkeletonCounter)
            {
                lastSkeletonFrame = SkeletonCounter;
                gestureRecognizer.Update(activeSkeleton);
            }
        }

        public Vector3 GetActiveSkeletonPoint(JointType type)
        {
            if (activeSkeleton != null)
            {
                return ToVector3(activeSkeleton.Joints[type].Position);
            }

            return Vector3.Zero;
        }

        public Vector3 GetWorkFistPosition(FistType fistType)
        {
            JointType hType;

            if (activeSkeleton != null && activeFistTrackingMode != FistTrackingMode.None)
            {
                if (fistType == FistType.Left)
                {
                    hType = JointType.HandLeft;
                }
                else
                {
                    hType = JointType.HandRight;
                }

                SkeletonPoint point = activeSkeleton.Joints[hType].Position;
                Vector3 returnPoint = new Vector3();

                if (fistType == FistType.Left && 
                    (activeFistTrackingMode == FistTrackingMode.Left ||
                    activeFistTrackingMode == FistTrackingMode.LeftPositionOnly ||
                    activeFistTrackingMode == FistTrackingMode.BothPositionOnly))
                {
                    returnPoint.X = /*Clamp(*/positionLeftAX * point.X + positionLeftBX/*, minX, maxX)*/;
                    returnPoint.Y = /*Clamp(*/positionLeftAY * point.Y + positionLeftBY/*, minY, maxY)*/;
                    returnPoint.Z = /*Clamp(*/positionLeftAZ * point.Z + positionLeftBZ/*, minZ, maxZ)*/;
                }
                else if (activeFistTrackingMode == FistTrackingMode.Right ||
                    activeFistTrackingMode == FistTrackingMode.RightPositionOnly ||
                    activeFistTrackingMode == FistTrackingMode.BothPositionOnly)
                {
                    returnPoint.X = /*Clamp(*/positionRightAX * point.X + positionRightBX/*, minX, maxX)*/;
                    returnPoint.Y = /*Clamp(*/positionRightAY * point.Y + positionRightBY/*, minY, maxY)*/;
                    returnPoint.Z = /*Clamp(*/positionRightAZ * point.Z + positionRightBZ/*, minZ, maxZ)*/;
                }

                return returnPoint;
            }
            else
            {
                return new Vector3();
            }
        }

        public Vector3 GetWorkFistPosition(FistType fistType, Vector3 jointPosition)
        {
            if (activeSkeleton != null && activeFistTrackingMode != FistTrackingMode.None)
            {
                Vector3 returnPoint = new Vector3();

                if (fistType == FistType.Left &&
                    (activeFistTrackingMode == FistTrackingMode.Left ||
                    activeFistTrackingMode == FistTrackingMode.LeftPositionOnly ||
                    activeFistTrackingMode == FistTrackingMode.BothPositionOnly))
                {
                    returnPoint.X = /*Clamp(*/positionLeftAX * jointPosition.X + positionLeftBX/*, minX, maxX)*/;
                    returnPoint.Y = /*Clamp(*/positionLeftAY * jointPosition.Y + positionLeftBY/*, minY, maxY)*/;
                    returnPoint.Z = /*Clamp(*/positionLeftAZ * jointPosition.Z + positionLeftBZ/*, minZ, maxZ)*/;
                }
                else if (activeFistTrackingMode == FistTrackingMode.Right ||
                    activeFistTrackingMode == FistTrackingMode.RightPositionOnly ||
                    activeFistTrackingMode == FistTrackingMode.BothPositionOnly)
                {
                    returnPoint.X = /*Clamp(*/positionRightAX * jointPosition.X + positionRightBX/*, minX, maxX)*/;
                    returnPoint.Y = /*Clamp(*/positionRightAY * jointPosition.Y + positionRightBY/*, minY, maxY)*/;
                    returnPoint.Z = /*Clamp(*/positionRightAZ * jointPosition.Z + positionRightBZ/*, minZ, maxZ)*/;
                }

                return returnPoint;
            }
            else
            {
                return new Vector3();
            }
        }        
        
        public float GetObjectScale()
        {
            float scale = 1;

            if (activeSkeleton != null && !translationGestureEnable && !rotationGestureEnable)
            {
                Vector3 leftHandPosition = GetActiveSkeletonPoint(JointType.HandLeft);
                Vector3 rightHandPosition = GetActiveSkeletonPoint(JointType.HandRight);

                float spiney = activeSkeleton.Joints[JointType.Spine].Position.Y;

                float distance = Vector3.Distance(leftHandPosition, rightHandPosition);

                if (!scaleGestureEnable && distance > 0.0f && (distance > 0.8f || distance < 0.25f) && 
                    Math.Abs(distance - lastDistance) < 0.1f &&
                    leftHandPosition.Y > spiney && rightHandPosition.Y > spiney &&
                    DateTime.Now - lastScaleGestureAbleTime > scaleGestureInactiveTime)
                {
                    if (DateTime.Now - lastScaleGestureStableTime > scaleGestureStabilizationTime)
                    {
                        scaleGestureEnable = true;
                        distanceSG = distance;
                        lastScaleGestureStableTime = DateTime.Now;
                    }
                }
                else
                {
                    if (!scaleGestureEnable)
                    {
                        lastScaleGestureStableTime = DateTime.Now;
                    }
                }

                if (scaleGestureEnable && Math.Abs(distance - lastDistance) < 0.03f)
                {
                    if (DateTime.Now - lastScaleGestureStableTime > scaleGestureStabilizationTime)
                    {
                        scaleGestureEnable = false;
                        lastScaleGestureStableTime = DateTime.Now;

                        lastScaleGestureAbleTime = DateTime.Now;
                        lastRotationGestureAbleTime = DateTime.Now;
                        lastTranslationGestureAbleTime = DateTime.Now;
                    }
                }
                else
                {
                    if (scaleGestureEnable)
                    {
                        lastScaleGestureStableTime = DateTime.Now;
                    }                    
                }

                lastDistance = distance;

                if (scaleGestureEnable)
                {
                    scale = distanceSG - distance;
                }
            }

            return scale;
        }

        public Vector3 GetObjectRotation()
        {
            Vector3 rotation = new Vector3();

            if (activeSkeleton != null && !translationGestureEnable && !scaleGestureEnable)
            {
                Vector3 leftHandPosition = GetActiveSkeletonPoint(JointType.HandLeft);
                Vector3 rightHandPosition = GetActiveSkeletonPoint(JointType.HandRight);

                float spiney = activeSkeleton.Joints[JointType.Spine].Position.Y;

                float distance = Vector3.Distance(leftHandPosition, rightHandPosition);

                if (distance > 0.25 && distance < 0.8 &&
                    DateTime.Now - lastRotationGestureAbleTime > rotationGestureInactiveTime)
                {
                    float lDistance = Vector3.Distance(lastLeftHandPositionRG, leftHandPosition);
                    float rDistance = Vector3.Distance(lastRightHandPositionRG, rightHandPosition);

                    if (!rotationGestureEnable && lDistance < 0.1 && rDistance < 0.1 &&
                        leftHandPosition.Y > spiney && rightHandPosition.Y > spiney)
                    {
                        if (DateTime.Now - lastRotationGestureStableTime > rotationGestureStabilizationTime)
                        {
                            rotationGestureEnable = true;
                            leftHandPositionRG = leftHandPosition;
                            rightHandPositionRG = rightHandPosition;
                            sphereCenterRG = (leftHandPositionRG + rightHandPositionRG) / 2;

                            if (Math.Abs(leftHandPosition.X - rightHandPosition.X) < 0.2)
                            {
                                onlyx = true;
                            }

                            lastax = 0;
                            lastay = 0;
                            lastaz = 0;
                        }
                    }
                    else
                    {
                        lastRotationGestureStableTime = DateTime.Now;
                    }
                }
                else
                {
                    if (rotationGestureEnable)
                    {
                        lastTranslationGestureAbleTime = DateTime.Now;
                        lastRotationGestureAbleTime = DateTime.Now;
                        lastScaleGestureAbleTime = DateTime.Now;
                    }

                    rotationGestureEnable = false;
                    onlyx = false; onlyy = false; onlyz = false;
                    lastRotationGestureStableTime = DateTime.Now;
                }

                lastLeftHandPositionRG = leftHandPosition;
                lastRightHandPositionRG = rightHandPosition;

                if (rotationGestureEnable)
                {
                    //policz srodek
                    Vector3 actualCenter = (leftHandPosition + rightHandPosition) / 2;

                    //przesun pozycje, tak aby srodek byl ten sam
                    Vector3 translation = sphereCenterRG - actualCenter;

                    Vector3 newLeftPos = leftHandPosition + translation;

                    //przesuniecie wektorow do 0.0.0
                    Vector3 tmpLeft = newLeftPos - sphereCenterRG;

                    Vector3 refLeft = leftHandPositionRG - sphereCenterRG;

                    Vector2 zrottmp = new Vector2(tmpLeft.X, tmpLeft.Y);
                    Vector2 zrotref = new Vector2(refLeft.X, refLeft.Y);
                    Vector2 yrottmp = new Vector2(tmpLeft.X, tmpLeft.Z);
                    Vector2 yrotref = new Vector2(refLeft.X, refLeft.Z);
                    Vector2 xrottmp = new Vector2(tmpLeft.X, tmpLeft.Y);
                    Vector2 xrotref = new Vector2(refLeft.X, refLeft.Y);

                    zrottmp.Normalize();
                    zrotref.Normalize();
                    yrottmp.Normalize();
                    yrotref.Normalize();
                    xrottmp.Normalize();
                    xrotref.Normalize();

                    int signz = 0;
                    int signy = 0;
                    int signx = 0;

                    if (zrottmp.Y < zrotref.Y)
                    {
                        signz = 1;
                    }
                    else
                    {
                        signz = -1;
                    }
                    if (yrottmp.Y > yrotref.Y)
                    {
                        signy = 1;
                    }
                    else
                    {
                        signy = -1;
                    }
                    if (xrottmp.Y < xrotref.Y)
                    {
                        signx = 1;
                    }
                    else
                    {
                        signx = -1;
                    }

                    float anglex = (float)Math.Acos((double)Vector2.Dot(xrottmp, xrotref));
                    float angley = (float)Math.Acos((double)Vector2.Dot(yrottmp, yrotref));
                    float anglez = (float)Math.Acos((double)Vector2.Dot(zrottmp, zrotref));

                    if (!(onlyx || onlyy || onlyz))
                    {
                        if (MathHelper.ToDegrees(Math.Abs(angley - lastay)) > 10)
                        {
                            onlyy = true;
                        }
                        if (MathHelper.ToDegrees(Math.Abs(anglez - lastaz)) > 10)
                        {
                            onlyz = true;
                        }
                    }

                    lastax = anglex;
                    lastay = angley;
                    lastaz = anglez;

                    if (onlyx)
                    {
                        return new Vector3(RotateFactor * anglex * signx, 0, 0);
                    }
                    if (onlyy)
                    {
                        return new Vector3(0, RotateFactor * angley * signy, 0);
                    }
                    if (onlyz)
                    {
                        return new Vector3(0, 0, RotateFactor * anglez * signz);
                    }

                    rotation = new Vector3(0, RotateFactor * signy * angley, RotateFactor * signz * anglez);
                }

            }

            return rotation;
        }

        private float rotateFactor = 1.5f;

        public float RotateFactor
        {
            get { return rotateFactor; }
            set { rotateFactor = value; }
        }


        public Vector3 GetObjectTranslation(FistType hand)
        {
            Vector3 translation = new Vector3();

            if (activeSkeleton != null && !rotationGestureEnable && !scaleGestureEnable)
            {               
                Vector3 leftHandPosition = GetActiveSkeletonPoint(JointType.HandLeft);
                Vector3 rightHandPosition = GetActiveSkeletonPoint(JointType.HandRight);

                float spiney = activeSkeleton.Joints[JointType.Spine].Position.Y;

                float distance = Vector3.Distance(leftHandPosition, rightHandPosition);

                if (distance > 0.0f && distance < 0.25f && !mayBeTranslationChange &&
                DateTime.Now - lastTranslationGestureAbleTime > translationGestureInactiveTime)
                {
                    if (!translationGestureEnable &&
                        leftHandPosition.Y > spiney &&
                        rightHandPosition.Y > spiney)
                    {
                        mayBeTranslationChange = true;
                        lastTranslationGestureStableTime = DateTime.Now;
                    }
                    if (translationGestureEnable)
                    {
                        mayBeTranslationChange = true;
                        lastTranslationGestureStableTime = DateTime.Now;
                    }
                }

                if (mayBeTranslationChange)
                {
                    if (DateTime.Now - lastTranslationGestureStableTime < translationGestureStabilizationTime)
                    {
                        if (distance > 0.25f)
                        {
                            translationGestureEnable = !translationGestureEnable;

                            if (translationGestureEnable)
                            {
                                leftHandPositionTG = leftHandPosition;
                                rightHandPositionTG = rightHandPosition;
                            }
                            else
                            {
                                lastRotationGestureAbleTime = DateTime.Now;
                                lastScaleGestureAbleTime = DateTime.Now;
                            }

                            lastTranslationGestureAbleTime = DateTime.Now;
                            mayBeTranslationChange = false;
                        }
                    }
                    else
                    {
                        lastTranslationGestureAbleTime = DateTime.Now;
                        mayBeTranslationChange = false;
                    }
                }

                if (translationGestureEnable)
                {
                    if (hand == FistType.Left)
                    {
                        translation = GetWorkFistPosition(FistType.Left) - GetWorkFistPosition(FistType.Left, leftHandPositionTG);
                    }
                    else
                    {
                        translation = GetWorkFistPosition(FistType.Right) - GetWorkFistPosition(FistType.Right, rightHandPositionTG);
                    }

                    Clamp(translation.X, minX, maxX);
                    Clamp(translation.Y, minY, maxY);
                    Clamp(translation.Z, minZ, maxZ);
                }
            }

            return translation;
        }        

        #endregion

        #region Working Threads

        private void CheckStatus()
        {
            while (true)
            {
                /*while (kinect == null)
                {
                    foreach (KinectSensor possibleKinect in KinectSensor.KinectSensors)
                    {
                        if (possibleKinect.Status == KinectStatus.Connected)
                        {
                            kinect = possibleKinect;
                            coordMapper = new CoordinateMapper(kinect);

                            TransformSmoothParameters smoothingParam = new TransformSmoothParameters();
                            {
                                smoothingParam.Smoothing = 0.5f;
                                smoothingParam.Correction = 0.1f;
                                smoothingParam.Prediction = 0.5f;
                                smoothingParam.JitterRadius = 0.1f;
                                smoothingParam.MaxDeviationRadius = 0.1f;
                            };

                            kinect.SkeletonStream.Enable();//smoothingParam);
                            kinect.ColorStream.Disable();
                            kinect.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);

                            skeletons = new Skeleton[kinect.SkeletonStream.FrameSkeletonArrayLength];
                            depthData = new short[kinect.DepthStream.FramePixelDataLength];

                            kinect.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(kinect_AllFramesReady);
                            //kinect.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(kinect_DepthFrameReady);
                            //kinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(kinect_SkeletonFrameReady);

                            kinect.Start();

                            speechRecognizer.SetAudioSource(kinect.AudioSource);
                            break;
                        }
                    }
                }*/

                if (kinect.Status == KinectStatus.Connected)
                {
                    kinectIsConnected = true;
                }
                else
                {
                    calibrationFinish = false;
                    findSkeleton = false;
                    leftFistCalibrationFinish = false;
                    rightFistCalibrationFinish = false;
                    kinectIsConnected = false;
                    kinect.AudioSource.Stop();
                    kinect.Stop();
                    leftFistCalculator.Abort();
                    rightFistCalculator.Abort();
                    kinect = null;
                }

                if (kinect != null && kinectIsConnected && !calibrationFinish)
                {
                    activeSkeleton = null;

                    if (leftFistCalculator != null)
                    {
                        leftFistCalculator.Abort();
                    }

                    if (rightFistCalculator != null)
                    {
                        rightFistCalculator.Abort();
                    }

                    leftFistCalculator = new Thread(new ThreadStart(CalculateLeftFist));
                    rightFistCalculator = new Thread(new ThreadStart(CalculateRightFist));

                    if (activeFistTrackingMode != FistTrackingMode.None)
                    {
                        if (activeFistTrackingMode == FistTrackingMode.Left)
                        {
                            leftFistCalculator.Start();
                        }
                        
                        if (activeFistTrackingMode == FistTrackingMode.Right)
                        {
                            rightFistCalculator.Start();
                        }
                    }                    

                    CalibrateKinectOnce();
                }
            }
        }

        private void CalculateLeftFist()
        {
            while (true)
            {
                if (activeFistTrackingMode == FistTrackingMode.Left && newLeftFrame)
                {
                    newLeftFrame = false;
                    leftFistArea = GetAreaOfFist(FistType.Left, out leftFistAreaTexture);

                    if (leftFistArea < 0.1f)
                    {
                        leftFistLastStates[LeftFistLastStatesCounter] = FistState.Undefined;
                        LeftFistLastStatesCounter++;
                    }
                    else
                    {
                        float closedAreaAtZ = GetClosedAreaAtZ(FistType.Left, leftHandZ);
                        float openedAreaAtZ = GetOpenedAreaAtZ(FistType.Left, leftHandZ);

                        if (leftFistArea < (closedAreaAtZ + closedAreaAtZ * 0.1f))
                        {
                            leftFistLastStates[LeftFistLastStatesCounter] = FistState.Close;
                            LeftFistLastStatesCounter++;
                        }
                        else
                        {
                            leftFistLastStates[LeftFistLastStatesCounter] = FistState.Open;
                            LeftFistLastStatesCounter++;
                        }
                    }

                    int closed = 0, opened = 0, undefined = 0;

                    for (int i = 0; i < leftFistLastStates.Length; i++)
                    {
                        if (leftFistLastStates[i] == FistState.Open)
                        {
                            opened++;
                            continue;
                        }

                        if (leftFistLastStates[i] == FistState.Close)
                        {
                            closed++;
                            continue;
                        }

                        if (leftFistLastStates[i] == FistState.Undefined)
                        {
                            undefined++;
                            continue;
                        }
                    }

                    if (closed >= opened)
                    {
                        leftFistState = FistState.Close;

                        if (undefined > closed)
                        {
                            leftFistState = FistState.Undefined;
                        }
                    }
                    else
                    {
                        leftFistState = FistState.Open;
                        if (undefined > opened)
                        {
                            leftFistState = FistState.Undefined;
                        }
                    }
                }
            }
        }

        private void CalculateRightFist()
        {
            while (true)
            {
                if (activeFistTrackingMode == FistTrackingMode.Right && newRightFrame)
                {
                    newRightFrame = false;
                    rightFistArea = GetAreaOfFist(FistType.Right, out rightFistAreaTexture);

                    if (rightFistArea < 0.1f)
                    {
                        rightFistLastStates[RightFistLastStatesCounter] = FistState.Undefined;
                        RightFistLastStatesCounter++;
                    }
                    else
                    {
                        float openedAreaAtZ = GetOpenedAreaAtZ(FistType.Right, rightHandZ);
                        float closedAreaAtZ = GetClosedAreaAtZ(FistType.Right, rightHandZ);

                        if (rightFistArea < (closedAreaAtZ + closedAreaAtZ * 0.1f))
                        {
                            rightFistLastStates[RightFistLastStatesCounter] = FistState.Close;
                            RightFistLastStatesCounter++;
                        }
                        else
                        {
                            rightFistLastStates[RightFistLastStatesCounter] = FistState.Open;
                            RightFistLastStatesCounter++;
                        }
                    }

                    int closed = 0, opened = 0, undefined = 0;

                    for (int i = 0; i < rightFistLastStates.Length; i++)
                    {
                        if (rightFistLastStates[i] == FistState.Open)
                        {
                            opened++;
                            continue;
                        }

                        if (rightFistLastStates[i] == FistState.Close)
                        {
                            closed++;
                            continue;
                        }

                        if (rightFistLastStates[i] == FistState.Undefined)
                        {
                            undefined++;
                            continue;
                        }
                    }

                    if (closed >= opened)
                    {
                        rightFistState = FistState.Close;

                        if (undefined > closed)
                        {
                            rightFistState = FistState.Undefined;
                        }
                    }
                    else
                    {
                        rightFistState = FistState.Open;
                        if (undefined > opened)
                        {
                            rightFistState = FistState.Undefined;
                        }
                    }
                }
            }
        }

        #endregion
        
        #region Utils

        public static Vector3 ToVector3(SkeletonPoint vector)
        {
            return new Vector3(vector.X, vector.Y, vector.Z);
        }

        public static float Clamp(float val, float min, float max)
        {
            if (val.CompareTo(min) < 0) return min;
            else if (val.CompareTo(max) > 0) return max;
            else return val;
        }

        private float GetAreaOfFist(FistType type)
        {
            Bitmap tmp = null;
            return GetAreaOfFist(type, out tmp);
        }        

        private float GetAreaOfFist(FistType type, out Bitmap textureOfArea)
        {
            if (activeSkeleton != null)
            {
                JointType handType, wristType, elbowType;

                if (type == FistType.Right)
                {
                    handType = JointType.HandRight;
                    wristType = JointType.WristRight;
                    elbowType = JointType.ElbowRight;
                }
                else
                {
                    handType = JointType.HandLeft;
                    wristType = JointType.WristLeft;
                    elbowType = JointType.ElbowLeft;
                }

                if (activeSkeleton.Joints[handType].TrackingState != JointTrackingState.Tracked)
                //if (activeSkeleton.Joints[handType].TrackingState == JointTrackingState.NotTracked)
                {
                    textureOfArea = null;
                    return 0;
                }
                else if(activeSkeleton.Joints[handType].TrackingState == JointTrackingState.Tracked)
                {
                    if (type == FistType.Right)
                    {
                        rightHandZ = activeSkeleton.Joints[handType].Position.Z;
                    }
                    else
                    {
                        leftHandZ = activeSkeleton.Joints[handType].Position.Z;
                    }
                }

                int depthWidth = kinect.DepthStream.FrameWidth;
                int depthHeight = kinect.DepthStream.FrameHeight;

                DepthImagePoint handDepthPoint = coordMapper.MapSkeletonPointToDepthPoint(activeSkeleton.Joints[handType].Position, kinect.DepthStream.Format);//DepthImageFormat.Resolution640x480Fps30);

                if (handDepthPoint.X < dxHalf || handDepthPoint.Y < dyHalf || handDepthPoint.X >= depthWidth - dxHalf || handDepthPoint.Y >= depthHeight - dyHalf)
                {
                    textureOfArea = null;
                    return 0;
                }

                Vector2 handDir = new Vector2();
                bool wristTracked = false;
                float c = 0;
                DepthImagePoint wristDepthPoint = new DepthImagePoint();
                DepthImagePoint elbowDepthPoint = new DepthImagePoint();

                //if (activeSkeleton.Joints[wristType].TrackingState == JointTrackingState.Tracked && activeSkeleton.Joints[elbowType].TrackingState == JointTrackingState.Tracked)
                if (activeSkeleton.Joints[wristType].TrackingState != JointTrackingState.NotTracked && activeSkeleton.Joints[elbowType].TrackingState != JointTrackingState.NotTracked)
                {
                    wristDepthPoint = coordMapper.MapSkeletonPointToDepthPoint(activeSkeleton.Joints[wristType].Position, kinect.DepthStream.Format);
                    elbowDepthPoint = coordMapper.MapSkeletonPointToDepthPoint(activeSkeleton.Joints[elbowType].Position, kinect.DepthStream.Format);

                    /*handDir = new Point(wristDepthPoint.X - elbowDepthPoint.X, wristDepthPoint.Y - elbowDepthPoint.Y);// - new Point(elbowDepthPoint.X, elbowDepthPoint.Y);
                    Normalize(handDir);*/

                    handDir = new Vector2(wristDepthPoint.X, wristDepthPoint.Y) - new Vector2(elbowDepthPoint.X, elbowDepthPoint.Y);
                    handDir.Normalize();                    

                    c = -(handDir.X * wristDepthPoint.X) - (handDir.Y * wristDepthPoint.Y) + 10;
                    wristTracked = true;
                }
                else
                {
                    textureOfArea = null;
                    return 0;
                }

                Bitmap cutBitmap = new Bitmap(dxHalf * 2 + 1, dyHalf * 2 + 1);

                int trackedJointIndex = depthWidth * handDepthPoint.Y + handDepthPoint.X;

                int depth = depthData[trackedJointIndex] >> 3;
                byte trackedJoinR = (byte)(255 - (depth >> 4));
                
                int row = 0;
                int col = 0;
                for (int i = trackedJointIndex - dxHalf - (dyHalf * depthWidth), j = 0; j < (dxHalf * 2 + 1) * (dyHalf * 2 + 1); j++, i++, col++)
                {
                    if (col % (dxHalf * 2 + 1) == 0)
                    {
                        col = 0;
                    }

                    if (wristTracked && -(handDir.X * (col + wristDepthPoint.X - dxHalf)) - (handDir.Y * (row + wristDepthPoint.Y - dyHalf)) > c)
                    {
                        cutBitmap.SetPixel(col, row, System.Drawing.Color.FromArgb(255, 0, 0, 0));
                    }
                    else
                    {
                        depth = depthData[i] >> 3;
                        byte depthByte = (byte)(255 - (depth >> 4));

                        cutBitmap.SetPixel(col, row, System.Drawing.Color.FromArgb(255, depthByte, depthByte, depthByte));
                    }

                    if (j != 0 && j % (dxHalf * 2 + 1) == 0)
                    {
                        i -= (dxHalf * 2 + 1);
                        i += depthWidth;

                        row++;
                    }
                }

                Image<Bgr, Byte> depthImgFromBitmap = new Image<Bgr, Byte>(cutBitmap);
                Image<Gray, Byte> gray = depthImgFromBitmap.Convert<Gray, Byte>().PyrDown().PyrUp();

                gray = gray.SmoothBlur(5, 5);

                gray = gray.ThresholdBinary(new Gray(trackedJoinR - 4), new Gray(trackedJoinR + 4));

                //gray = gray.Canny(new Gray(255), new Gray(10));
                textureOfArea = gray.ToBitmap();               

                Contour<System.Drawing.Point> cont = gray.FindContours(CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_NONE, RETR_TYPE.CV_RETR_TREE);

                if (cont != null)
                {
                    Seq<System.Drawing.Point> hull = cont.GetConvexHull(ORIENTATION.CV_COUNTER_CLOCKWISE);

                    return (float)hull.Area;
                }

                return 0;
            }

            textureOfArea = null;
            return 0;
        }

        private void TrackClosestSkeleton()
        {
            if (this.kinect != null && this.kinect.SkeletonStream != null)
            {
                if (!this.kinect.SkeletonStream.AppChoosesSkeletons)
                {
                    this.kinect.SkeletonStream.AppChoosesSkeletons = true;
                }

                float closestDistance = 10000f;
                int closestID = 0;

                foreach (Skeleton skeleton in this.skeletons)
                {
                    if (skeleton.TrackingState != SkeletonTrackingState.NotTracked && skeleton.Position.Z < closestDistance)
                    {
                        closestID = skeleton.TrackingId;
                        closestDistance = skeleton.Position.Z;

                        if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                        {
                            activeSkeleton = skeleton;
                        }
                    }
                }

                if (closestID > 0)
                {
                    this.kinect.SkeletonStream.ChooseSkeletons(closestID);
                }
            }
        }

        private void CalibrateKinectOnce()
        {
            if (kinect.Status != KinectStatus.Connected)
            {
                return;
            }
            
            #region Finding Skeleton
            
            JointType jointWhere0 = JointType.ShoulderCenter;
            float jointWhere0PositionY;

            //findSkeleton = true;

            if (!findSkeleton)
            {
                kinect.ElevationAngle = kinect.MaxElevationAngle; 
            }

            while (!findSkeleton)
            {
                if (lastSkeletonFrame != SkeletonCounter)
                {
                    lastSkeletonFrame = SkeletonCounter;
                    if (activeSkeleton != null)
                    {
                        if (activeSkeleton.Joints[jointWhere0].TrackingState == JointTrackingState.Tracked)
                        {
                            jointWhere0PositionY = activeSkeleton.Joints[jointWhere0].Position.Y;

                            if (jointWhere0PositionY < 0)
                            {
                                System.Threading.Thread.Sleep(1000);
                                if (kinect.ElevationAngle - 4 > kinect.MinElevationAngle)
                                {
                                    kinect.ElevationAngle = kinect.ElevationAngle - 4;
                                }
                                else
                                {
                                    findSkeleton = true;
                                }
                            }
                            else
                            {
                                lastCalibraionTime = DateTime.Now;
                                findSkeleton = true;
                            }
                        }
                    }
                    else
                    {
                        System.Threading.Thread.Sleep(1000);
                        if (kinect.ElevationAngle - 4 > kinect.MinElevationAngle)
                        {
                            kinect.ElevationAngle = kinect.ElevationAngle - 4;
                        }
                        else
                        {
                            findSkeleton = true;
                        }
                    }
                }
            }

            #endregion
            
            SkeletonPoint minPositionPoint = new SkeletonPoint(), maxPositionPoint = new SkeletonPoint();            

            whatIsCalibrated = 1;
            calibrationBufferCounter = 0;

            rightFistCalibrationActive = false;

            if (activeFistTrackingMode == FistTrackingMode.Right)
            {
                #region Right Fist Calibration - With Fist State

                while (!rightFistCalibrationFinish)
                {
                    if (lastDepthFrame != DepthCounter &&
                    DateTime.Now - lastCalibraionTime > betweenCalibrationTime)
                    {
                        rightFistCalibrationActive = true;
                        lastDepthFrame = DepthCounter;
                        if (activeSkeleton != null)
                        {
                            if (activeSkeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked &&
                                activeSkeleton.Joints[JointType.HandRight].Position.Y > activeSkeleton.Joints[JointType.ElbowRight].Position.Y
                                )
                            {
                                SkeletonPoint point = activeSkeleton.Joints[JointType.HandRight].Position;
                                float rightHandZ = point.Z;

                                if (rightHandZ < minRightZ)
                                {
                                    minPositionPoint = point;
                                    minRightZ = rightHandZ;
                                }
                                if (rightHandZ > maxRightZ)
                                {
                                    maxPositionPoint = point;
                                    maxRightZ = rightHandZ;
                                }

                                if (rightFistArea > 0)
                                {
                                    if (calibrationBufferCounter == calibrationBufferLength)
                                    {
                                        float area = 0;
                                        Array.Sort(calibrationBuffer);

                                        for (int i = calibrationBufferDrop - 1; i < calibrationBufferLength - calibrationBufferDrop - 1; i++)
                                        {
                                            area += calibrationBuffer[i];
                                        }

                                        calibrationBufferCounter = 0;
                                        Array.Clear(calibrationBuffer, 0, calibrationBufferLength);

                                        switch (whatIsCalibrated)
                                        {
                                            case 1:
                                                {
                                                    farOpenedRightArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 2:
                                                {
                                                    farClosedRightArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 3:
                                                {
                                                    nearOpenedRightArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 4:
                                                {
                                                    nearClosedRightArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);

                                                    openedRightA = (farOpenedRightArea - nearOpenedRightArea) / (minRightZ - maxRightZ);
                                                    openedRightB = nearOpenedRightArea - openedRightA * maxRightZ;

                                                    closedRightA = (farClosedRightArea - nearClosedRightArea) / (minRightZ - maxRightZ);
                                                    closedRightB = nearClosedRightArea - closedRightA * maxRightZ;

                                                    rightFistCalibrationFinish = true;
                                                    break;
                                                }
                                        }

                                        lastCalibraionTime = DateTime.Now;
                                        rightFistCalibrationActive = false;

                                        if (rightFistCalibrationFinish)
                                        {
                                            float halfBetweenMinZMaxZ = Math.Abs(maxRightZ - minRightZ) / 2;

                                            if (minPositionPoint.X > maxPositionPoint.X)
                                            {
                                                realRightMinX = maxPositionPoint.X - halfBetweenMinZMaxZ;
                                                realRightMaxX = minPositionPoint.X + halfBetweenMinZMaxZ;
                                            }
                                            else
                                            {
                                                realRightMinX = minPositionPoint.X - halfBetweenMinZMaxZ;
                                                realRightMaxX = maxPositionPoint.X + halfBetweenMinZMaxZ;
                                            }

                                            if (minPositionPoint.Y > maxPositionPoint.Y)
                                            {
                                                realRightMinY = maxPositionPoint.Y - halfBetweenMinZMaxZ;
                                                realRightMaxY = minPositionPoint.Y + halfBetweenMinZMaxZ;
                                            }
                                            else
                                            {
                                                realRightMinY = minPositionPoint.Y - halfBetweenMinZMaxZ;
                                                realRightMaxY = maxPositionPoint.Y + halfBetweenMinZMaxZ;
                                            }

                                            realRightMinZ = minRightZ;
                                            realRightMaxZ = maxRightZ;

                                            positionRightAX = (minX - maxX) / (realRightMinX - realRightMaxX);
                                            positionRightBX = maxX - positionRightAX * realRightMaxX;
                                            positionRightAY = (minY - maxY) / (realRightMinY - realRightMaxY);
                                            positionRightBY = maxY - positionRightAY * realRightMaxY;
                                            positionRightAZ = (minZ - maxZ) / (realRightMinZ - realRightMaxZ);
                                            positionRightBZ = maxZ - positionRightAZ * realRightMaxZ;
                                        }

                                        whatIsCalibrated++;
                                    }

                                    calibrationBuffer[calibrationBufferCounter++] = rightFistArea;
                                }
                            }
                        }
                    }
                }

                #endregion
            }
            whatIsCalibrated = 1;
            calibrationBufferCounter = 0;

            leftFistCalibrationActive = false;
            if (activeFistTrackingMode == FistTrackingMode.Left)
            {
                #region Left Fist Calibration - With Fist State

                while (activeFistTrackingMode != FistTrackingMode.None && activeFistTrackingMode != FistTrackingMode.Right && !leftFistCalibrationFinish)
                {
                    if (lastDepthFrame != DepthCounter &&
                    DateTime.Now - lastCalibraionTime > betweenCalibrationTime)
                    {
                        leftFistCalibrationActive = true;
                        lastDepthFrame = DepthCounter;
                        if (activeSkeleton != null)
                        {
                            if (activeSkeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked &&
                                activeSkeleton.Joints[JointType.HandLeft].Position.Y > activeSkeleton.Joints[JointType.ElbowLeft].Position.Y
                                )
                            {
                                SkeletonPoint point = activeSkeleton.Joints[JointType.HandLeft].Position;
                                float leftHandZ = point.Z;

                                if (leftHandZ < minLeftZ)
                                {
                                    minPositionPoint = point;
                                    minLeftZ = leftHandZ;
                                }
                                if (leftHandZ > maxLeftZ)
                                {
                                    maxPositionPoint = point;
                                    maxLeftZ = leftHandZ;
                                }

                                if (leftFistArea > 0)
                                {
                                    if (calibrationBufferCounter == calibrationBufferLength)
                                    {
                                        float area = 0;
                                        Array.Sort(calibrationBuffer);

                                        for (int i = calibrationBufferDrop - 1; i < calibrationBufferLength - calibrationBufferDrop - 1; i++)
                                        {
                                            area += calibrationBuffer[i];
                                        }

                                        calibrationBufferCounter = 0;
                                        Array.Clear(calibrationBuffer, 0, calibrationBufferLength);

                                        switch (whatIsCalibrated)
                                        {
                                            case 1:
                                                {
                                                    farOpenedLeftArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 2:
                                                {
                                                    farClosedLeftArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 3:
                                                {
                                                    nearOpenedLeftArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);
                                                    break;
                                                }
                                            case 4:
                                                {
                                                    nearClosedLeftArea = area / (calibrationBufferLength - 2 * calibrationBufferDrop);

                                                    openedLeftA = (farOpenedLeftArea - nearOpenedLeftArea) / (minLeftZ - maxLeftZ);
                                                    openedLeftB = nearOpenedLeftArea - openedLeftA * maxLeftZ;

                                                    closedLeftA = (farClosedLeftArea - nearClosedLeftArea) / (minLeftZ - maxLeftZ);
                                                    closedLeftB = nearClosedLeftArea - closedLeftA * maxLeftZ;

                                                    leftFistCalibrationFinish = true;
                                                    break;
                                                }
                                        }

                                        lastCalibraionTime = DateTime.Now;
                                        leftFistCalibrationActive = false;

                                        if (leftFistCalibrationFinish)
                                        {
                                            float halfBetweenMinZMaxZ = Math.Abs(maxLeftZ - minLeftZ) / 2;

                                            if (minPositionPoint.X > maxPositionPoint.X)
                                            {
                                                realLeftMinX = maxPositionPoint.X - halfBetweenMinZMaxZ;
                                                realLeftMaxX = minPositionPoint.X + halfBetweenMinZMaxZ;
                                            }
                                            else
                                            {
                                                realLeftMaxX = maxPositionPoint.X + halfBetweenMinZMaxZ;
                                                realLeftMinX = minPositionPoint.X - halfBetweenMinZMaxZ;
                                            }

                                            if (minPositionPoint.Y > maxPositionPoint.Y)
                                            {
                                                realLeftMinY = maxPositionPoint.Y - halfBetweenMinZMaxZ;
                                                realLeftMaxY = minPositionPoint.Y + halfBetweenMinZMaxZ;
                                            }
                                            else
                                            {
                                                realLeftMinY = minPositionPoint.Y - halfBetweenMinZMaxZ;
                                                realLeftMaxY = maxPositionPoint.Y + halfBetweenMinZMaxZ;
                                            }

                                            realLeftMinZ = minLeftZ;
                                            realLeftMaxZ = maxLeftZ;

                                            positionLeftAX = (minX - maxX) / (realLeftMinX - realLeftMaxX);
                                            positionLeftBX = maxX - positionLeftAX * realLeftMaxX;
                                            positionLeftAY = (minY - maxY) / (realLeftMinY - realLeftMaxY);
                                            positionLeftBY = maxY - positionLeftAY * realLeftMaxY;
                                            positionLeftAZ = (minZ - maxZ) / (realLeftMinZ - realLeftMaxZ);
                                            positionLeftBZ = maxZ - positionLeftAZ * realLeftMaxZ;
                                        }

                                        whatIsCalibrated++;
                                    }

                                    calibrationBuffer[calibrationBufferCounter++] = leftFistArea;
                                }
                            }
                        }
                    }
                }

                #endregion
            }

            TimeSpan shortCalibrationTimeSpan = new TimeSpan(0, 0, 5);
            DateTime shortCalibrationStart;

            if (activeFistTrackingMode == FistTrackingMode.RightPositionOnly ||
                activeFistTrackingMode == FistTrackingMode.BothPositionOnly)
            {
                #region Right Fist Calibration - Position Only

                shortCalibrationStart = DateTime.Now;

                while (!rightFistCalibrationFinish &&
                    DateTime.Now - shortCalibrationStart < shortCalibrationTimeSpan)
                {
                    if (activeSkeleton != null && lastSkeletonFrame != SkeletonCounter)
                    {
                        lastSkeletonFrame = SkeletonCounter;
                        if (activeSkeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked &&
                            activeSkeleton.Joints[JointType.HandRight].Position.Y > activeSkeleton.Joints[JointType.ElbowRight].Position.Y
                            )
                        {
                            SkeletonPoint point = activeSkeleton.Joints[JointType.HandRight].Position;
                            float rightHandZ = point.Z;

                            if (rightHandZ < minRightZ)
                            {
                                minPositionPoint = point;
                                minRightZ = rightHandZ;
                            }
                            if (rightHandZ > maxRightZ)
                            {
                                maxPositionPoint = point;
                                maxRightZ = rightHandZ;
                            }
                        }
                    }
                }

                float halfBetweenMinZMaxZ = Math.Abs(maxRightZ - minRightZ) / 2;

                if (minPositionPoint.X > maxPositionPoint.X)
                {
                    realRightMinX = maxPositionPoint.X - halfBetweenMinZMaxZ;
                    realRightMaxX = minPositionPoint.X + halfBetweenMinZMaxZ;
                }
                else
                {
                    realRightMaxX = maxPositionPoint.X + halfBetweenMinZMaxZ;
                    realRightMinX = minPositionPoint.X - halfBetweenMinZMaxZ;
                }

                if (minPositionPoint.Y > maxPositionPoint.Y)
                {
                    realRightMinY = maxPositionPoint.Y - halfBetweenMinZMaxZ;
                    realRightMaxY = minPositionPoint.Y + halfBetweenMinZMaxZ;
                }
                else
                {
                    realRightMinY = minPositionPoint.Y - halfBetweenMinZMaxZ;
                    realRightMaxY = maxPositionPoint.Y + halfBetweenMinZMaxZ;
                }

                realRightMinZ = minRightZ;
                realRightMaxZ = maxRightZ;

                positionRightAX = (minX - maxX) / (realRightMinX - realRightMaxX);
                positionRightBX = maxX - positionRightAX * realRightMaxX;
                positionRightAY = (minY - maxY) / (realRightMinY - realRightMaxY);
                positionRightBY = maxY - positionRightAY * realRightMaxY;
                positionRightAZ = (minZ - maxZ) / (realRightMinZ - realRightMaxZ);
                positionRightBZ = maxZ - positionRightAZ * realRightMaxZ;

                rightFistCalibrationFinish = true;

                #endregion
            }

            if (activeFistTrackingMode == FistTrackingMode.LeftPositionOnly ||
                activeFistTrackingMode == FistTrackingMode.BothPositionOnly)
            {
                #region Left Fist Calibration - Position Only
                
                shortCalibrationStart = DateTime.Now;

                while (!leftFistCalibrationFinish &&
                    DateTime.Now - shortCalibrationStart < shortCalibrationTimeSpan)
                {
                    if (activeSkeleton != null && lastSkeletonFrame != SkeletonCounter)
                    {
                        lastSkeletonFrame = SkeletonCounter;
                        if (activeSkeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked &&
                            activeSkeleton.Joints[JointType.HandLeft].Position.Y > activeSkeleton.Joints[JointType.ElbowLeft].Position.Y
                            )
                        {
                            SkeletonPoint point = activeSkeleton.Joints[JointType.HandLeft].Position;
                            float leftHandZ = point.Z;

                            if (leftHandZ < minLeftZ)
                            {
                                minPositionPoint = point;
                                minLeftZ = leftHandZ;
                            }
                            if (leftHandZ > maxLeftZ)
                            {
                                maxPositionPoint = point;
                                maxLeftZ = leftHandZ;
                            }
                        }
                    }
                }

                float halfBetweenMinZMaxZ = Math.Abs(maxLeftZ - minLeftZ) / 2;

                if (minPositionPoint.X > maxPositionPoint.X)
                {
                    realLeftMinX = maxPositionPoint.X - halfBetweenMinZMaxZ;
                    realLeftMaxX = minPositionPoint.X + halfBetweenMinZMaxZ;
                }
                else
                {
                    realLeftMaxX = maxPositionPoint.X + halfBetweenMinZMaxZ;
                    realLeftMinX = minPositionPoint.X - halfBetweenMinZMaxZ;
                }

                if (minPositionPoint.Y > maxPositionPoint.Y)
                {
                    realLeftMinY = maxPositionPoint.Y - halfBetweenMinZMaxZ;
                    realLeftMaxY = minPositionPoint.Y + halfBetweenMinZMaxZ;
                }
                else
                {
                    realLeftMinY = minPositionPoint.Y - halfBetweenMinZMaxZ;
                    realLeftMaxY = maxPositionPoint.Y + halfBetweenMinZMaxZ;
                }

                realLeftMinZ = minLeftZ;
                realLeftMaxZ = maxLeftZ;

                positionLeftAX = (minX - maxX) / (realLeftMinX - realLeftMaxX);
                positionLeftBX = maxX - positionLeftAX * realLeftMaxX;
                positionLeftAY = (minY - maxY) / (realLeftMinY - realLeftMaxY);
                positionLeftBY = maxY - positionLeftAY * realLeftMaxY;
                positionLeftAZ = (minZ - maxZ) / (realLeftMinZ - realLeftMaxZ);
                positionLeftBZ = maxZ - positionLeftAZ * realLeftMaxZ;

                leftFistCalibrationFinish = true;

                #endregion
            }            

            calibrationFinish = true;
        }

        private float GetOpenedAreaAtZ(FistType fistType, float z)
        {
            if (activeFistTrackingMode != FistTrackingMode.None)
            {
                if (fistType == FistType.Left && activeFistTrackingMode == FistTrackingMode.Left)
                {
                    return openedLeftA * z + openedLeftB;
                }
                else if (activeFistTrackingMode == FistTrackingMode.Right)
                {
                    return openedRightA * z + openedRightB;
                }
                else
                {
                    return 0;
                }
            }
            else
            {
                return 0;
            }
        }

        private float GetClosedAreaAtZ(FistType fistType, float z)
        {
            if (activeFistTrackingMode != FistTrackingMode.None)
            {
                if (fistType == FistType.Left && activeFistTrackingMode == FistTrackingMode.Left)
                {
                    return closedLeftA * z + closedLeftB;
                }
                else if (activeFistTrackingMode == FistTrackingMode.Right)
                {
                    return closedRightA * z + closedRightB;
                }
                else
                {
                    return 0;
                }
            }
            else
            {
                return 0;
            }
        }
        
        #endregion

        #region Events

        private void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    skeletonFrame.CopySkeletonDataTo(this.skeletons);
                    TrackClosestSkeleton();                    

                    SkeletonCounter++;
                }
            }
        }

        private void kinect_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {
                if (depthFrame != null)
                {
                    DepthCounter++;
                    depthFrame.CopyPixelDataTo(depthData);
                    newRightFrame = true;
                    newLeftFrame = true;
                }
            }
        }

        private void kinect_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
            {
                if (colorFrame == null)
                    return;

                if (colorData == null)
                {
                    colorData = new byte[colorFrame.Width * colorFrame.Height * 4];
                }

                colorFrame.CopyPixelDataTo(colorData);
            }
        }

        private void kinect_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {                
                if (skeletonFrame != null)
                {
                    skeletonFrame.CopySkeletonDataTo(this.skeletons);
                    TrackClosestSkeleton();

                    if (activeSkeleton != null)
                    {
                        gestureRecognizer.Update(activeSkeleton);
                    }

                    SkeletonCounter++;
                }
            }

            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {                
                if (depthFrame != null)
                {
                    DepthCounter++;
                    depthFrame.CopyPixelDataTo(depthData);
                    newRightFrame = true;
                    newLeftFrame = true;
                }
            }
        }

        private void leftHandAboveHead_OnGestureSucces(GestureEventArgs args)
        {
            leftHandAboveHead.Disable();
            leftHandBelowShoulder.Enable();

            if (!rotationGestureEnable)
            {
                translationGestureEnable = !translationGestureEnable;
            }
        }

        private void leftHandBelowShoulder_OnGestureSucces(GestureEventArgs args)
        {
            leftHandBelowShoulder.Disable();
            leftHandAboveHead.Enable();
        }

        private void speechRecognizer_MOVE(object sender, EventArgs e)
        {
            if (!rotationGestureEnable)
            {
                translationGestureEnable = true;
            }
        }

        private void speechRecognizer_OK(object sender, EventArgs e)
        {
            if (translationGestureEnable)
            {
                translationGestureEnable = false;
            }
            else if (rotationGestureEnable)
            {
                rotationGestureEnable = false;
            }
        }

        #endregion
    }
}
